#ifndef IR_AVOIDANCE_H
#define IR_AVOIDANCE_H

#define FRONT_LEFT_IR_AVOIDANCE_PIN 15
#define FRONT_RIGHT_IR_AVOIDANCE_PIN 23

#include <Arduino.h>
#include "component_btn7919.h"

#define IR_AVOIDANCE_STOP 0

int front_left_ir_avoidance;
int front_right_ir_avoidance;

// avoidance interval
long previousMillis_ir_avoidance;
const int interval_ir_avoidance = 200;

void setup_ir_avoidance()
{
  pinMode(FRONT_LEFT_IR_AVOIDANCE_PIN, INPUT);
  pinMode(FRONT_RIGHT_IR_AVOIDANCE_PIN, INPUT);
  front_left_ir_avoidance = 0;
  front_right_ir_avoidance = 0;
}

void loop_ir_avoidance()
{
  unsigned long currentMillis = millis();
  if (currentMillis - previousMillis_ir_avoidance >= interval_ir_avoidance)
  {
    previousMillis_ir_avoidance = currentMillis; // Remember the time
    front_left_ir_avoidance = digitalRead(FRONT_LEFT_IR_AVOIDANCE_PIN);
    front_right_ir_avoidance = digitalRead(FRONT_RIGHT_IR_AVOIDANCE_PIN);

    Serial.println(front_left_ir_avoidance);
    Serial.println(front_right_ir_avoidance);
    if ((front_left_ir_avoidance == IR_AVOIDANCE_STOP) || (front_right_ir_avoidance == IR_AVOIDANCE_STOP))
    {
      if (car_state != ROBOT_BACKWARD)
      {
        car_action(ROBOT_STOP);
      }
    }
  }
}

#endif // IR_AVOIDANCE_H
